// modbus_tool.cpp
#include "modbus_tool.h"
#include <iostream>
#include <sstream>
#include <iomanip>

ModbusTool::ModbusTool() {}

ModbusTool::~ModbusTool() {
    stop();
}

bool ModbusTool::start(const std::string& port, int baud_rate) {
    if (running_) {
        return false;
    }
    serial_ = std::make_unique<SerialInterface>();
    try {
        if (!serial_->open(port, baud_rate)) {
            return false;
        }
    } catch (const std::system_error& e) {
        std::cerr << "Error opening serial port: " << e.what() << std::endl;
        return false;
    }
    running_ = true;
    receive_thread_ = std::thread(&ModbusTool::receive_thread_func, this);

    return true;
}

void ModbusTool::stop() {
    if (!running_) {
        return;
    }

    running_ = false;
    
    if (receive_thread_.joinable()) {
        receive_thread_.join();
    }
    
    if (serial_) {
        serial_->close();
    }
}

void ModbusTool::query_registers(uint8_t machine_addr,uint16_t start_addr, uint16_t end_addr) {
    if (start_addr > end_addr) {
        std::cerr << "Error: Invalid register range" << std::endl;
        return;
    }
    
    uint16_t reg_count = end_addr - start_addr + 1;
    std::cout << "Querying registers 0x" << std::hex << std::setw(4) << std::setfill('0') 
              << start_addr << "-0x" << end_addr << " (" << std::dec << reg_count 
              << " registers)..." << std::endl;
    
    auto request = create_request(READ_HOLDING_REGISTERS, machine_addr,start_addr, reg_count);
    if (serial_->write(request.data(), request.size()) != static_cast<ssize_t>(request.size())) {
        std::cerr << "Failed to send query" << std::endl;
    }

    // 打印发送的数据
    std::ostringstream oss;
    oss << "[TX] ";
    for (const auto& byte : request) {
        oss << std::hex << std::setw(2) << std::setfill('0') << static_cast<int>(byte) << " ";
    }
    std::cout << oss.str() << std::endl;
}

void ModbusTool::write_single_register(uint8_t machine_addr,uint16_t addr, uint16_t value) {
    if (addr <= 0) {
        std::cout<<"Invalid register range: start > end"<<std::endl;
        return;
    }
    
    std::cout << "Writing single register 0x" << std::hex << addr 
              << " with value 0x" << value << "..." << std::endl;
    
    auto request = create_request(WRITE_SINGLE_REGISTER, machine_addr, addr, 1, {value});
    if (serial_->write(request.data(), request.size()) != static_cast<ssize_t>(request.size())) {
        std::cout << "Failed to send write request" << std::endl;
    }
    // 打印发送的数据
    std::ostringstream oss;
    oss << "[TX] ";
    for (const auto& byte : request) {
        oss << std::hex << std::setw(2) << std::setfill('0') << static_cast<int>(byte) << " ";
    }
    std::cout << oss.str() << std::endl;
}

void ModbusTool::write_multiple_registers(uint8_t machine_addr, uint16_t start_addr, const std::vector<uint16_t>& values) {
    if (start_addr <= 0) {
        std::cout<<"Invalid register range: start > end"<<std::endl;
        return;
    }
    std::cout << "Writing " << std::dec << values.size() << " registers starting at 0x" 
              << std::hex << start_addr << "..." << std::endl;
    std::cout << "Values: ";
    for (const auto& value : values) {
        std::cout << "0x" << std::hex << value << " ";
    }
    std::cout << std::endl;
    
    auto request = create_request(WRITE_MULTIPLE_REGISTERS, machine_addr,start_addr, values.size(), values);
    if (serial_->write(request.data(), request.size()) != static_cast<ssize_t>(request.size())) {
        std::cerr << "Failed to send write request" << std::endl;
    }

    // 打印发送的数据
    std::ostringstream oss;
    oss << "[TX] ";
    for (const auto& byte : request) {
        oss << std::hex << std::setw(2) << std::setfill('0') << static_cast<int>(byte) << " ";
    }
    std::cout << oss.str() << std::endl;
}

void ModbusTool::receive_thread_func() {
    constexpr size_t BUFFER_SIZE = 256;
    std::array<uint8_t, BUFFER_SIZE> buffer;
    
    std::cout << "Receive thread started (Thread ID: " 
              << std::this_thread::get_id() << ")" << std::endl;
    
    while (running_) {
        try {
            // 非阻塞读取数据
            ssize_t bytes_read = serial_->read(buffer.data(), buffer.size());
            
            if (bytes_read > 0) {
                // 打印接收到的原始数据
                print_hex_data("RX", buffer.data(), bytes_read);

                // 解析Modbus响应
                parse_response(buffer.data(), static_cast<size_t>(bytes_read));
            } 
            else if (bytes_read == 0) {
                // 无数据可用，短暂休眠避免CPU占用过高
                std::this_thread::sleep_for(std::chrono::milliseconds(10));
            }
            else {
                // 处理读取错误 (非EAGAIN/EWOULDBLOCK错误)
                if (errno != EAGAIN && errno != EWOULDBLOCK) {
                    std::lock_guard<std::mutex> lock(serial_mutex_);
                    std::cerr << "[ERROR] Serial read failed: " 
                              << std::strerror(errno) << " (errno: " << errno << ")\n";
                    break;
                }
            }
        }
        catch (const std::exception& e) {
            std::lock_guard<std::mutex> lock(serial_mutex_);
            std::cerr << "[EXCEPTION] In receive thread: " << e.what() << "\n";
            running_ = false;
            break;
        }
    }
    
    std::cout << "Receive thread exiting (Thread ID: " 
              << std::this_thread::get_id() << ")" << std::endl;
}

void ModbusTool::parse_response(const uint8_t* buffer, size_t length) {
    if (length < 5) {
        return;
    }

    uint8_t func = buffer[1];

    if (func & 0x80) {
        // 错误响应
        uint8_t error_code = buffer[2];
        std::cerr << "Error response: code=0x" << std::hex << static_cast<int>(error_code) << std::endl;
        return;
    }

    switch (func) {
        case READ_HOLDING_REGISTERS: {
            query_cb_(buffer,length);
            break;
        }
        
        case WRITE_SINGLE_REGISTER: {
            single_cb_(buffer,length);
            break;
        }
        
        case WRITE_MULTIPLE_REGISTERS: {
            multiple_cb_(buffer,length);
            break;
        }
        
        default:
            std::cerr << "Unsupported function code in response: 0x" 
                      << std::hex << static_cast<int>(func) << std::endl;
            break;
    }
}
